﻿//-------------------------------------------------------------------
//	Copyright (c) 2010-2014  Zhirnov Andrey
//	This file is part of the "UX Standard Lib" project.
//	See copyright notice in "UX_STL_Ext.h".
//-------------------------------------------------------------------

#ifndef _UXS_TRANSFORM_EXTENDED_H
#define _UXS_TRANSFORM_EXTENDED_H

#pragma once

#include "MathTypesExt.h"
#include "TTransform.h"

namespace UX_STL
{
namespace UXMathExt
{

	//
	// Transform (with matrix cashe)
	//

	template <typename T>
	struct TTransformExt
	{
	private:
		TMatrix<T,4,4>	_sMatrix;
		TQuaternion<T>	_sOrient;
		TVec<T,3>		_sPos;
		bool			_bMatrixValid;

	public:
		TTransformExt() {}
		TTransformExt(const TTransform<T> &tr);
		TTransformExt(const TTransformExt<T> &tr);
		TTransformExt(const TMatrix<T,4,4> &mat);
		TTransformExt(const T *pos, const T *orient);
		TTransformExt(const TVec<T,3> &pos, const TQuaternion<T> &orient);

		TVec<T,3>			& Position()										{ return _sPos; }
		TQuaternion<T>		& Orientation()										{ return _sOrient; }

		const TVec<T,3>	&	  Position()		const							{ return _sPos; }
		const TQuaternion<T>& Orientation()		const							{ return _sOrient; }
		
		TTransformExt<T>   operator - ()		const							{ return GetInverse(); }

		TTransformExt<T> & operator += (const TTransformExt<T> &right);
		TTransformExt<T> & operator -= (const TTransformExt<T> &right);

		TTransformExt<T>   operator +  (const TTransformExt<T> &right)	const;
		TTransformExt<T>   operator -  (const TTransformExt<T> &right)	const;

		bool			   operator == (const TTransformExt<T> &right)	const;
		bool			   operator != (const TTransformExt<T> &right)	const;

		TTransform<T>	   Add(const TTransformExt<T> &right)	const;
		TTransform<T>	   Sub(const TTransformExt<T> &right)	const;

		void			   Set(const TVec<T,3> &pos, const TQuaternion<T> &orient);

		TTransformExt<T> & Move(const TVec<T,3> &delta);
		TTransformExt<T> & Rotate(const TQuaternion<T> &delta);

		TTransformExt<T> & Inverse();
		TTransformExt<T>   GetInverse()	const;
		
		const TMatrix<T,4,4>& GetMatrix();

		TVec<T,3>		GetGlobalVector(const TVec<T,3> &sLocal)	const;
		TVec<T,3>		GetLocalVector(const TVec<T,3> &sGlobal)	const;

		TVec<T,3>		GetGlobalPos(const TVec<T,3> &sLocal)		const;
		TVec<T,3>		GetLocalPos(const TVec<T,3> &sGlobal)		const;


		template <typename T2>
		TTransformExt<T2>	Convert() const;

		TTransform<T>		ToTransform()	const;

		string				ToString()		const;
		string				ToStringFull()	const;
	};


	
	
	template <typename T>
	inline TTransformExt<T>::TTransformExt(const T *pos, const T *orient):
		_sPos(pos), _sOrient(orient), _sMatrix(), _bMatrixValid(false)
	{}
	

	template <typename T>
	inline TTransformExt<T>::TTransformExt(const TTransform<T> &tr):
		_sPos(tr._sPos), _sOrient(tr._sOrient), _sMatrix(), _bMatrixValid(false)
	{}


	template <typename T>
	inline TTransformExt<T>::TTransformExt(const TTransformExt &tr):
		_sPos(tr._sPos), _sOrient(tr._sOrient), _sMatrix(tr._sMatrix), _bMatrixValid(tr._bMatrixValid)
	{}
	

	template <typename T>
	inline TTransformExt<T>::TTransformExt(const TVec<T,3> &pos, const TQuaternion<T> &orient):
		_sPos(pos), _sOrient(orient), _sMatrix(), _bMatrixValid(false)
	{}


	template <typename T>
	inline TTransformExt<T>::TTransformExt(const TMatrix<T,4,4> &mat):
		_sPos( mat.GetTranslation() ), _sOrient( mat ), _sMatrix(mat), _bMatrixValid(true)
	{}
	
	
	template <typename T>
	inline TTransformExt<T> & TTransformExt<T>::operator += (const TTransformExt<T> &right)
	{
		_sPos	 += _sOrient * right._sPos;
		_sOrient *= right._sOrient;
		_bMatrixValid = false;
		return *this;
	}

	
	template <typename T>
	inline TTransformExt<T> & TTransformExt<T>::operator -= (const TTransformExt<T> &right)
	{
		return *this += right.GetInverse();
	}

	
	template <typename T>
	inline TTransformExt<T> TTransformExt<T>::operator + (const TTransformExt<T> &right) const
	{
		return TTransformExt<T>( *this ) += right;
	}

	
	template <typename T>
	inline TTransformExt<T> TTransformExt<T>::operator - (const TTransformExt<T> &right) const
	{
		return TTransformExt<T>( *this ) -= right;
	}
	
	
	template <typename T>
	inline TTransform<T> TTransformExt<T>::Add(const TTransformExt<T> &right) const
	{
		return TTransform<T>( _sPos, _sOrient ) += TTransform<T>( right._sPos, right._sOrient );
	}

	
	template <typename T>
	inline TTransform<T> TTransformExt<T>::Sub(const TTransformExt<T> &right) const
	{
		return TTransform<T>( _sPos, _sOrient ) -= TTransform<T>( right._sPos, right._sOrient );
	}

		
	template <typename T>
	inline bool TTransformExt<T>::operator == (const TTransformExt<T> &right) const
	{
		return ( _sPos == right._sPos and _sOrient == right._sOrient );
	}
	
	
	template <typename T>
	inline bool TTransformExt<T>::operator != (const TTransformExt<T> &right) const
	{
		return not ( *this == right );
	}

	
	template <typename T>
	inline void TTransformExt<T>::Set(const TVec<T,3> &pos, const TQuaternion<T> &orient)
	{
		_sPos = pos;
		_sOrient = orient;
		_bMatrixValid = false;
	}


	template <typename T>
	inline const TMatrix<T,4,4> & TTransformExt<T>::GetMatrix()
	{
		if ( !_bMatrixValid ) {
			_sMatrix	  = _sOrient.GetMatrix( _sPos );
			_bMatrixValid = true;
		}
		return _sMatrix;
	}


	template <typename T>
	inline TTransformExt<T> & TTransformExt<T>::Inverse()
	{
		_sOrient.Inverse();
		_sPos = _sOrient * -_sPos;
		_bMatrixValid = false;
		return *this;
	}


	template <typename T>
	inline TTransformExt<T> TTransformExt<T>::GetInverse() const
	{
		return TTransformExt( *this ).Inverse();
	}
	

	template <typename T>
	inline TTransformExt<T> & TTransformExt<T>::Move(const TVec<T,3> &delta)
	{
		_sPos += _sOrient * delta;
		_bMatrixValid = false;
		return *this;
	}

	
	template <typename T>
	inline TTransformExt<T> & TTransformExt<T>::Rotate(const TQuaternion<T> &delta)
	{
		_sOrient *= delta;
		_bMatrixValid = false;
		return *this;
	}


	template <typename T>
	inline TVec<T,3> TTransformExt<T>::GetGlobalVector(const TVec<T,3> &sLocal) const
	{
		return ( _sOrient * sLocal );
	}

		
	template <typename T>
	inline TVec<T,3> TTransformExt<T>::GetGlobalPos(const TVec<T,3> &sLocal) const
	{
		return ( _sOrient * sLocal ) + _sPos;
	}

		
	template <typename T>
	inline TVec<T,3> TTransformExt<T>::GetLocalVector(const TVec<T,3> &sGlobal) const
	{
		return ( _sOrient.GetInverse() * sGlobal );
	}

		
	template <typename T>
	inline TVec<T,3> TTransformExt<T>::GetLocalPos(const TVec<T,3> &sGlobal) const
	{
		return _sOrient.GetInverse() * ( sGlobal - _sPos );
	}

	
	template <typename T>
	template <typename T2>
	inline TTransformExt<T2> TTransformExt<T>::Convert() const
	{
		TTransformExt<T2>	temp;
		temp._sPos		= _sPos.Convert<T2>();
		temp._sOrient	= _sOrient.Convert<T2>();
		temp._sMatrix	= _sMatrix.Convert<T2>();
		temp._bMatrixValid = _bMatrixValid;
	}
	
		
	template <typename T>
	inline TTransform<T> TTransformExt<T>::ToTransform() const
	{
		return TTransform<T>( _sPos, _sOrient );
	}

	
	template <typename T>
	inline string TTransformExt<T>::ToString() const
	{
		return (string("pos( ") << _sPos.ToString() << " ), orient( " << _sOrient.ToString() << " )");
	}

	
	template <typename T>
	inline string TTransformExt<T>::ToStringFull() const
	{
		return (string("pos( ") << _sPos.ToStringFull() << " ), orient( " << _sOrient.ToStringFull() << " )");
	}

//-------------------------------------------------------------------

}	// UXMathExt
}	// UX_STL

#endif	// _UXS_TRANSFORM_EXTENDED_H